/********************************************************************
created:	2009/09/05
created:	5:9:2009   1:27
filename: 	qyCurve.h
author:		Xiong Jiayu
copyright:  
purpose:	
*********************************************************************/
#pragma once

#ifndef __QYCURVE_H__
#define __QYCURVE_H__

#include "qyMath.h"
#include "../memsys/qyArray.h"

/*
===========================================
class qyCurve
===========================================
*/
template <typename type>
class qyCurve
{
public:
						qyCurve( void );
    virtual             ~qyCurve();

    virtual int			AddValue( const float time, const type &value );
    virtual void		RemoveIndex( const int index ); 
    virtual void		Clear( void ) { values.clear(); times.clear(); currentIndex = -1; changed = true; }

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

    virtual bool		IsDone( const float time ) const;

    int					GetNumValues( void ) const { return values.size(); }
    void				SetValue( const int index, const type &value ) { values[index] = value; changed = true; }
    type				GetValue( const int index ) const { return values[index]; }
    type *				GetValueAddress( const int index ) { return &values[index]; }
    type &				GetValueReference( const int index ) { return values[index]; }
    const type &		GetValueReference( const int index ) const { return values[index]; }
    float				GetTime( const int index ) const { return times[index]; }

    float				GetLengthForTime( const float time ) const;
    float				GetTimeForLength( const float length, const float epsilon = 0.1f ) const;
    float				GetLengthBetweenKnots( const int i0, const int i1 ) const;

    void				MakeUniform( const float totalTime );
    void				SetConstantSpeed( const float totalTime );
    void				ShiftTime( const float deltaTime );
    void				Translate( const type &translation );

protected:
    typedef qyArray<float>  TimeList;
    typedef qyArray<type>   ValueList;  
    TimeList            times;          // knots
    ValueList           values;         // knot values
    mutable int			currentIndex;	// cached index for fast lookup
    mutable bool		changed;		// set whenever the curve changes

    int                 IndexForTime( const float time ) const;
    float				TimeForIndex( const int index ) const;
    type				ValueForIndex( const int index ) const;

    float				GetSpeed( const float time ) const;
    float				RombergIntegral( const float t0, const float t1, const int order ) const;
};

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE qyCurve<type>::qyCurve()
{
    currentIndex = -1;
    changed = false;
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE qyCurve<type>::~qyCurve()
{

}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE int qyCurve<type>::AddValue( const float time, const type &value )
{
    int index = IndexForTime( time );
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.insert( it, time );
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.insert( it, value );
    }
    return index;
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE void qyCurve<type>::RemoveIndex( const int index )
{
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.erase(it);
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.erase( it );
    }
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE type qyCurve<type>::GetCurrentValue( const float time ) const
{
    int i;
    i = IndexForTime( time );
    if ( i >= (int)values.size() )
    {
        return values[values.size()  - 1];
    }
    else
    {
        return values[i];
    }
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE type qyCurve<type>::GetCurrentFirstDerivative( const float time ) const
{
    return ( values[0] - values[0] );
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve<type>::GetCurrentSecondDerivative( const float time ) const 
{
    return ( values[0] - values[0] );
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE bool qyCurve<type>::IsDone( const float time ) const
{
    return ( time >= times[times.size() -1] );
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE float qyCurve<type>::GetSpeed( const float time ) const
{
    int i;
    float speed = 0.0f;
    type value;

    value = GetCurrentFirstDerivative( time );

    speed += value * value;
    return sqrt( speed );
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE float qyCurve<type>::RombergIntegral( const float t0, const float t1, const int order ) const
{
    int i, j, k, m, n;
    float sum, delta;
    float *temp[2];
    temp[0] = (float *) _alloca( order * sizeof( float ) );
    temp[1] = (float *) _alloca( order * sizeof( float ) );

    delta = t1 - t0;
    temp[0][0] = 0.5f * delta * ( GetSpeed( t0 ) + GetSpeed( t1 ) );

    for ( i = 2, m = 1; i <= order; i++, m *= 2, delta *= 0.5f ) {

        // approximate using the trapezoid rule
        sum = 0.0f;
        for ( j = 1; j <= m; j++ ) {
            sum += GetSpeed( t0 + delta * ( j - 0.5f ) );
        }

        // Richardson extrapolation
        temp[1][0] = 0.5f * ( temp[0][0] + delta * sum );
        for ( k = 1, n = 4; k < i; k++, n *= 4 ) {
            temp[1][k] = ( n * temp[1][k-1] - temp[0][k-1] ) / ( n - 1 );
        }

        for ( j = 0; j < i; j++ ) {
            temp[0][j] = temp[1][j];
        }
    }
    return temp[0][order-1];
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE float qyCurve<type>::GetLengthBetweenKnots( const int i0, const int i1 ) const
{
    float length = 0.0f;
    for ( int i = i0, i < i1; i++ )
    {
        length += RombergIntegral( times[i], times[i+1], 5 );
    }
    return length;
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE float qyCurve<type>::GetLengthForTime( const float time ) const
{
    float length = 0.0f;
    int index = IndexForTime( time );
    for ( int i = 0; i < index; i++ )
    {
        length += RombergIntegral( times[i], times[i+1], 5 );
    }
    length += RombergIntegral( times[index], time, 5 );
    return length;
}

//-----------------------------------------------------------------------
template < typename type >
QY_INLINE float qyCurve<type>::GetTimeForLength( const float length, const float epsilon /* = 0.1f */ ) const
{
    int i, index;
    float *accumLength, totalLength, len0, len1, t, diff;

    if ( length <= 0.0f ) 
    {
        return times[0];
    }

    accumLength = (float *) _alloca( values.size() * sizeof( float ) );
    totalLength = 0.0f;
    for ( index = 0; index < values.size() - 1; index++ ) 
    {
        totalLength += GetLengthBetweenKnots( index, index + 1 );
        accumLength[index] = totalLength;
        if ( length < accumLength[index] ) 
        {
            break;
        }
    }

    if ( index >= values.size() - 1 ) 
    {
        return times[times.size() - 1];
    }

    if ( index == 0 ) 
    {
        len0 = length;
        len1 = accumLength[0];
    } 
    else
    {
        len0 = length - accumLength[index-1];
        len1 = accumLength[index] - accumLength[index-1];
    }

    // invert the arc length integral using Newton's method
    t = ( times[index+1] - times[index] ) * len0 / len1;
    for ( i = 0; i < 32; i++ ) 
    {
        diff = RombergIntegral( times[index], times[index] + t, 5 ) - len0;
        if ( fabs( diff ) <= epsilon ) 
        {
            return times[index] + t;
        }
        t -= diff / GetSpeed( times[index] + t );
    }
    return times[index] + t;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve<type>::MakeUniform( const float totalTime )
{
    int i, n;
    n = times.size() - 1;
    for ( i = 0; i <= n; i++ )
    {
        times[i] = i * totalTime / n;
    }
    changed = true;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve<type>::SetConstantSpeed( const float totalTime )
{
    int i;
    float *length, totalLength, scale, t;

    length = (float *) _alloca( values.size() * sizeof( float ) );
    totalLength = 0.0f;
    for ( i = 0; i < values.size() - 1; i++ ) 
    {
        length[i] = GetLengthBetweenKnots( i, i + 1 );
        totalLength += length[i];
    }
    scale = totalTime / totalLength;
    for ( t = 0.0f, i = 0; i < times.size() - 1; i++ ) 
    {
        times[i] = t;
        t += scale * length[i];
    }
    times[times.size() - 1] = totalTime;
    changed = true;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve<type>::ShiftTime( const float deltaTime )
{
    for ( int i = 0; i < times.size(); i++ )
    {
        times[i] += deltaTime;
    }
    changed = true;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve<type>::Translate( const type &translation )
{
    for ( int i = 0; i < values.size(); i++ )
    {
        values[i] += translation;
    }
    changed = true;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE int qyCurve<type>::IndexForTime( const float time ) const
{
    int len, mid, offset, res;

    if ( currentIndex >= 0 && currentIndex <= (int)times.size() ) 
    {
        // use the cached index if it is still valid
        if ( currentIndex == 0 ) {
            if ( time <= times[currentIndex] ) 
            {
                return currentIndex;
            }
        } 
        else if ( currentIndex == times.size() ) 
        {
            if ( time > times[currentIndex-1] ) 
            {
                return currentIndex;
            }
        } 
        else if ( time > times[currentIndex-1] && time <= times[currentIndex] ) 
        {
            return currentIndex;
        } 
        else if ( time > times[currentIndex] && ( currentIndex+1 == times.size() || time <= times[currentIndex+1] ) ) 
        {
            // use the next index
            currentIndex++;
            return currentIndex;
        }
    }

    // use binary search to find the index for the given time
    len = (int)times.size();
    mid = len;
    offset = 0;
    res = 0;
    while( mid > 0 ) {
        mid = len >> 1;
        if ( time == times[offset+mid] ) {
            return offset+mid;
        } else if ( time > times[offset+mid] ) {
            offset += mid;
            len -= mid;
            res = 1;
        } else {
            len -= mid;
            res = 0;
        }
    }
    currentIndex = offset+res;
    return currentIndex;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve<type>::ValueForIndex( const int index ) const
{
    int n = values.size() - 1;

    if ( index < 0 )
    {
        return values[0] + index * ( values[1] - values[0] );
    }
    else if ( index > n )
    {
        return values[n] + ( index - n ) * ( values[n] - values[n-1] );
    }
    return values[index];
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE float qyCurve<type>::TimeForIndex( const int index ) const
{
    int n = times.size()-1;

    if ( index < 0 ) 
    {
        return times[0] + index * ( times[1] - times[0] );
    } 
    else if ( index > n ) 
    {
        return times[n] + ( index - n ) * ( times[n] - times[n-1] );
    }
    return times[index]; 
}

/*
===========================================
class qyCurve_Bezier
===========================================
*/
template< typename type >
class qyCurve_Bezier : public qyCurve<type>
{
public:
    qyCurve_Bezier( void );

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    void				Basis( const int order, const float t, float *bvals ) const;
    void				BasisFirstDerivative( const int order, const float t, float *bvals ) const;
    void				BasisSecondDerivative( const int order, const float t, float *bvals ) const;
};

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE qyCurve_Bezier<type>::qyCurve_Bezier()
{

}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_Bezier<type>::GetCurrentValue( const float time ) const
{
    int i;
    float *bvals;
    type v;

    bvals = (float *) _alloca( this->values.size() * sizeof( float ) );

    Basis( (int)this->values.size(), time, bvals );
    v = bvals[0] * this->values[0];
    for ( i = 1; i < (int)this->values.size(); i++ ) 
    {
        v += bvals[i] * this->values[i];
    }
    return v;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_Bezier<type>::GetCurrentFirstDerivative( const float time ) const
{
    int i;
    float *bvals, d;
    type v;

    bvals = (float *) _alloca( this->values.size() * sizeof( float ) );

    BasisFirstDerivative( (int)this->values.size(), time, bvals );
    v = bvals[0] * this->values[0];
    for ( i = 1; i < (int)this->values.size(); i++ ) {
        v += bvals[i] * this->values[i];
    }
    d = ( this->times[ this->times.size()-1] - this->times[0] );
    return ( (float) (this->values.size()-1) / d ) * v;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_Bezier<type>::GetCurrentSecondDerivative( const float time ) const
{
    int i;
    float *bvals, d;
    type v;

    bvals = (float *)_alloca( this->values.size() * sizeof( float ) );

    BasisSecondDerivative( (int)this->values.size(), time, bvals );
    v = bvals[0] * this->values[0];
    for ( i = 1; i < (int)this->values.size(); i++ ) 
    {
        v += bvals[i] * this->values[i];
    }
    d = ( this->times[ this->times.size()-1] - this->times[0] );
    return ( (float) (this->values.size()-2) * (this->values.size()-1) / ( d * d ) ) * v;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_Bezier<type>::Basis( const int order, const float t, float *bvals ) const
{
    int i, j, d;
    float *c, c1, c2, s, o, ps, po;

    bvals[0] = 1.0f;
    d = order - 1;
    if ( d <= 0 ) 
    {
        return;
    }

    c = (float *) _alloca( (d+1) * sizeof( float ) );
    s = (float) ( t - this->times[0] ) / ( this->times[ this->times.size()-1] - this->times[0] );
    o = 1.0f - s;
    ps = s;
    po = o;

    for ( i = 1; i < d; i++ ) 
    {
        c[i] = 1.0f;
    }
    for ( i = 1; i < d; i++ ) 
    {
        c[i-1] = 0.0f;
        c1 = c[i];
        c[i] = 1.0f;
        for ( j = i+1; j <= d; j++ ) 
        {
            c2 = c[j];
            c[j] = c1 + c[j-1];
            c1 = c2;
        }
        bvals[i] = c[d] * ps;
        ps *= s;
    }
    for ( i = d-1; i >= 0; i-- ) 
    {
        bvals[i] *= po;
        po *= o;
    }
    bvals[d] = ps;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_Bezier<type>::BasisFirstDerivative( const int order, const float t, float *bvals ) const {
    int i;

    Basis( order-1, t, bvals+1 );
    bvals[0] = 0.0f;
    for ( i = 0; i < order-1; i++ ) 
    {
        bvals[i] -= bvals[i+1];
    }
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_Bezier<type>::BasisSecondDerivative( const int order, const float t, float *bvals ) const {
    int i;

    BasisFirstDerivative( order-1, t, bvals+1 );
    bvals[0] = 0.0f;
    for ( i = 0; i < order-1; i++ ) 
    {
        bvals[i] -= bvals[i+1];
    }
}


/*
===========================================
class qyCurve_QuadraticBezier
===========================================
Quadratic Bezier Curve should always have exactly three knots
*/
template< typename type >
class qyCurve_QuadraticBezier : public qyCurve< type >
{
public:
    qyCurve_QuadraticBezier( void );

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    void				Basis( const float t, float *bvals ) const;
    void				BasisFirstDerivative( const float t, float *bvals ) const;
    void				BasisSecondDerivative( const float t, float *bvals ) const;
};

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE qyCurve_QuadraticBezier<type>::qyCurve_QuadraticBezier()
{

}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_QuadraticBezier<type>::GetCurrentValue( const float time ) const
{
    float bvals[3];
    assert( this->values.size() == 3 );
    Basis( time, bvals );
    return ( bvals[0] * this->values[0] + bvals[1] * this->values[1] + bvals[2] * this->values[2] );
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_QuadraticBezier<type>::GetCurrentFirstDerivative( const float time ) const
{
    float bvals[3], d;
    assert( this->values.size() == 3 );
    BasisFirstDerivative( time, bvals );
    d = ( this->times[2] - this->times[0] );
    return ( bvals[0] * this->values[0] + bvals[1] * this->values[1] + bvals[2] * this->values[2] ) / d;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_QuadraticBezier<type>::GetCurrentSecondDerivative( const float time ) const 
{
    float bvals[3], d;
    assert( this->values.size() == 3 );
    BasisSecondDerivative( time, bvals );
    d = ( this->times[2] - this->times[0] );
    return ( bvals[0] * this->values[0] + bvals[1] * this->values[1] + bvals[2] * this->values[2] ) / ( d * d );
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_QuadraticBezier<type>::Basis( const float t, float *bvals ) const {
    float s1 = (float) ( t - this->times[0] ) / ( this->times[2] - this->times[0] );
    float s2 = s1 * s1;
    bvals[0] = s2 - 2.0f * s1 + 1.0f;
    bvals[1] = -2.0f * s2 + 2.0f * s1;
    bvals[2] = s2;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_QuadraticBezier<type>::BasisFirstDerivative( const float t, float *bvals ) const 
{
    float s1 = (float) ( t - this->times[0] ) / ( this->times[2] - this->times[0] );
    bvals[0] = 2.0f * s1 - 2.0f;
    bvals[1] = -4.0f * s1 + 2.0f;
    bvals[2] = 2.0f * s1;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_QuadraticBezier<type>::BasisSecondDerivative( const float t, float *bvals ) const {
    float s1 = (float) ( t - this->times[0] ) / ( this->times[2] - this->times[0] );
    bvals[0] = 2.0f;
    bvals[1] = -4.0f;
    bvals[2] = 2.0f;
}

/*
===========================================
class qyCurve_CubicBezier
===========================================
Cubic Bezier Curve
should always have exactly four knots
*/
template< typename type >
class qyCurve_CubicBezier : public qyCurve<type> {
public:
                        qyCurve_CubicBezier( void );

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    void				Basis( const float t, float *bvals ) const;
    void				BasisFirstDerivative( const float t, float *bvals ) const;
    void				BasisSecondDerivative( const float t, float *bvals ) const;
};

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE qyCurve_CubicBezier<type>::qyCurve_CubicBezier( void ) 
{
}


//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_CubicBezier<type>::GetCurrentValue( const float time ) const 
{
    float bvals[4];
    assert( this->values.size() == 4 );
    Basis( time, bvals );
    return ( bvals[0] * this->values[0] + bvals[1] * this->values[1] + bvals[2] * this->values[2] + bvals[3] * this->values[3] );
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_CubicBezier<type>::GetCurrentFirstDerivative( const float time ) const 
{
    float bvals[4], d;
    assert( this->values.size() == 4 );
    BasisFirstDerivative( time, bvals );
    d = ( this->times[3] - this->times[0] );
    return ( bvals[0] * this->values[0] + bvals[1] * this->values[1] + bvals[2] * this->values[2] + bvals[3] * this->values[3] ) / d;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_CubicBezier<type>::GetCurrentSecondDerivative( const float time ) const {
    float bvals[4], d;
    assert( this->values.size() == 4 );
    BasisSecondDerivative( time, bvals );
    d = ( this->times[3] - this->times[0] );
    return ( bvals[0] * this->values[0] + bvals[1] * this->values[1] + bvals[2] * this->values[2] + bvals[3] * this->values[3] ) / ( d * d );
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_CubicBezier<type>::Basis( const float t, float *bvals ) const 
{
    float s1 = (float) ( t - this->times[0] ) / ( this->times[3] - this->times[0] );
    float s2 = s1 * s1;
    float s3 = s2 * s1;
    bvals[0] = -s3 + 3.0f * s2 - 3.0f * s1 + 1.0f;
    bvals[1] = 3.0f * s3 - 6.0f * s2 + 3.0f * s1;
    bvals[2] = -3.0f * s3 + 3.0f * s2;
    bvals[3] = s3;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_CubicBezier<type>::BasisFirstDerivative( const float t, float *bvals ) const 
{
    float s1 = (float) ( t - this->times[0] ) / ( this->times[3] - this->times[0] );
    float s2 = s1 * s1;
    bvals[0] = -3.0f * s2 + 6.0f * s1 - 3.0f;
    bvals[1] = 9.0f * s2 - 12.0f * s1 + 3.0f;
    bvals[2] = -9.0f * s2 + 6.0f * s1;
    bvals[3] = 3.0f * s2;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_CubicBezier<type>::BasisSecondDerivative( const float t, float *bvals ) const 
{
    float s1 = (float) ( t - this->times[0] ) / ( this->times[3] - this->times[0] );
    bvals[0] = -6.0f * s1 + 6.0f;
    bvals[1] = 18.0f * s1 - 12.0f;
    bvals[2] = -18.0f * s1 + 6.0f;
    bvals[3] = 6.0f * s1;
}

/*
===========================================
class qyCurve_Spline
===========================================
*/
template< typename type >
class qyCurve_Spline : public qyCurve<type> 
{
public:
    enum				boundary_t { BT_FREE, BT_CLAMPED, BT_CLOSED };

                        qyCurve_Spline( void );

    virtual bool		IsDone( const float time ) const;

    virtual void		SetBoundaryType( const boundary_t bt ) { boundaryType = bt; this->changed = true; }
    virtual boundary_t	GetBoundaryType( void ) const { return boundaryType; }

    virtual void		SetCloseTime( const float t ) { closeTime = t; this->changed = true; }
    virtual float		GetCloseTime( void ) { return boundaryType == BT_CLOSED ? closeTime : 0.0f; }

protected:
    boundary_t			boundaryType;
    float				closeTime;

    type				ValueForIndex( const int index ) const;
    float				TimeForIndex( const int index ) const;
    float				ClampedTime( const float t ) const;
};

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE qyCurve_Spline<type>::qyCurve_Spline( void ) 
{
    boundaryType = BT_FREE;
    closeTime = 0.0f;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_Spline<type>::ValueForIndex( const int index ) const 
{
    int n = (int)this->values.size()-1;

    if ( index < 0 ) 
    {
        if ( boundaryType == this->BT_CLOSED ) 
        {
            return this->values[ this->values.size() + index % this->values.size() ];
        }
        else 
        {
            return this->values[0] + (float)index * ( this->values[1] - this->values[0] );
        }
    }
    else if ( index > n ) 
    {
        if ( boundaryType == this->BT_CLOSED ) 
        {
            return this->values[ index % this->values.size() ];
        }
        else 
        {
            return this->values[n] + (float)(( index - n )) * ( this->values[n] - this->values[n-1] );
        }
    }
    return this->values[index];
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE float qyCurve_Spline<type>::TimeForIndex( const int index ) const 
{
    int n = (int)this->times.size()-1;

    if ( index < 0 ) 
    {
        if ( boundaryType == this->BT_CLOSED ) 
        {
            return ( index / this->times.size() ) * ( this->times[n] + closeTime ) - ( this->times[n] + closeTime - this->times[this->times.size() + index % this->times.size()] );
        }
        else 
        {
            return this->times[0] + index * ( this->times[1] - this->times[0] );
        }
    }
    else if ( index > n ) 
    {
        if ( boundaryType == this->BT_CLOSED ) 
        {
            return ( index / this->times.size() ) * ( this->times[n] + closeTime ) + this->times[index % this->times.size()];
        }
        else 
        {
            return this->times[n] + ( index - n ) * ( this->times[n] - this->times[n-1] );
        }
    }
    return this->times[index];
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE float qyCurve_Spline<type>::ClampedTime( const float t ) const 
{
    if ( boundaryType == BT_CLAMPED ) {
        if ( t < this->times[0] ) {
            return this->times[0];
        }
        else if ( t >= this->times[ this->times.size()-1 ] ) {
            return this->times[ this->times.size()-1 ];
        }
    }
    return t;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE bool qyCurve_Spline<type>::IsDone( const float time ) const 
{
    return ( boundaryType != this->BT_CLOSED && time >= this->times[ this->times.size() - 1 ] );
}

/*
===========================================
class qyCurve_CatmullRomSpline
===========================================
Uniform Cubic Interpolation Spline
the curve goes through all the knots
*/
template< typename type >
class qyCurve_CatmullRomSpline : public qyCurve_Spline<type> 
{
public:
                        qyCurve_CatmullRomSpline( void );

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    void				Basis( const int index, const float t, float *bvals ) const;
    void				BasisFirstDerivative( const int index, const float t, float *bvals ) const;
    void				BasisSecondDerivative( const int index, const float t, float *bvals ) const;
};

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE qyCurve_CatmullRomSpline<type>::qyCurve_CatmullRomSpline( void ) 
{
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_CatmullRomSpline<type>::GetCurrentValue( const float time ) const {
    int i, j, k;
    float bvals[4], clampedTime;
    type v;

    if ( this->times.size() == 1 ) 
    {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    Basis( i-1, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < 4; j++ ) {
        k = i + j - 2;
        v += bvals[j] * ValueForIndex( k );
    }
    return v;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_CatmullRomSpline<type>::GetCurrentFirstDerivative( const float time ) const {
    int i, j, k;
    float bvals[4], d, clampedTime;
    type v;

    if ( this->times.size() == 1 ) 
    {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    BasisFirstDerivative( i-1, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < 4; j++ ) 
    {
        k = i + j - 2;
        v += bvals[j] * ValueForIndex( k );
    }
    d = ( TimeForIndex( i ) - TimeForIndex( i-1 ) );
    return v / d;
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE type qyCurve_CatmullRomSpline<type>::GetCurrentSecondDerivative( const float time ) const {
    int i, j, k;
    float bvals[4], d, clampedTime;
    type v;

    if ( this->times.size() == 1 ) 
    {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    BasisSecondDerivative( i-1, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < 4; j++ ) 
    {
        k = i + j - 2;
        v += bvals[j] * ValueForIndex( k );
    }
    d = ( TimeForIndex( i ) - TimeForIndex( i-1 ) );
    return v / ( d * d );
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_CatmullRomSpline<type>::Basis( const int index, const float t, float *bvals ) const 
{
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = ( ( -s + 2.0f ) * s - 1.0f ) * s * 0.5f;				// -0.5f s * s * s + s * s - 0.5f * s
    bvals[1] = ( ( ( 3.0f * s - 5.0f ) * s ) * s + 2.0f ) * 0.5f;	// 1.5f * s * s * s - 2.5f * s * s + 1.0f
    bvals[2] = ( ( -3.0f * s + 4.0f ) * s + 1.0f ) * s * 0.5f;		// -1.5f * s * s * s - 2.0f * s * s + 0.5f s
    bvals[3] = ( ( s - 1.0f ) * s * s ) * 0.5f;						// 0.5f * s * s * s - 0.5f * s * s
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_CatmullRomSpline<type>::BasisFirstDerivative( const int index, const float t, float *bvals ) const 
{
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = ( -1.5f * s + 2.0f ) * s - 0.5f;						// -1.5f * s * s + 2.0f * s - 0.5f
    bvals[1] = ( 4.5f * s - 5.0f ) * s;								// 4.5f * s * s - 5.0f * s
    bvals[2] = ( -4.5f * s + 4.0f ) * s + 0.5f;						// -4.5 * s * s + 4.0f * s + 0.5f
    bvals[3] = 1.5f * s * s - s;									// 1.5f * s * s - s
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_CatmullRomSpline<type>::BasisSecondDerivative( const int index, const float t, float *bvals ) const 
{
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = -3.0f * s + 2.0f;
    bvals[1] = 9.0f * s - 5.0f;
    bvals[2] = -9.0f * s + 4.0f;
    bvals[3] = 3.0f * s - 1.0f;
}

/*
===========================================
class qyCurve_KochanekBartelsSpline
===========================================
cubic interpolating spline
the curve goes through all the knots
the curve becomes the Catmull-Rom spline if the tension,
continuity and bias are all set to zero
*/
template< typename type >
class qyCurve_KochanekBartelsSpline : public qyCurve_Spline<type> 
{
public:
                        qyCurve_KochanekBartelsSpline( void );

    virtual int			AddValue( const float time, const type &value );
    virtual int			AddValue( const float time, const type &value, const float tension, const float continuity, const float bias );
    virtual void		RemoveIndex( const int index ); 
    virtual void		Clear( void ) { this->values.clear(); this->times.clear(); tension.clear(); continuity.clear(); bias.clear(); this->currentIndex = -1; }

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    qyArray<float>		tension;
    qyArray<float>		continuity;
    qyArray<float>		bias;

    void				TangentsForIndex( const int index, type &t0, type &t1 ) const;

    void				Basis( const int index, const float t, float *bvals ) const;
    void				BasisFirstDerivative( const int index, const float t, float *bvals ) const;
    void				BasisSecondDerivative( const int index, const float t, float *bvals ) const;
};

/*
====================
qyCurve_KochanekBartelsSpline::qyCurve_KochanekBartelsSpline
====================
*/
template< typename type >
QY_INLINE qyCurve_KochanekBartelsSpline<type>::qyCurve_KochanekBartelsSpline( void ) {
}


//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_KochanekBartelsSpline<type>::RemoveIndex( const int index )
{
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.erase(it);
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.erase( it );
    }
    {
        qyArray<float>::iterator it = tension.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        tension.erase( it );
    }
    {
        qyArray<float>::iterator it = continuity.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        continuity.erase( it );
    }
    {
        qyArray<float>::iterator it = bias.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        bias.erase( it );
    }
}

/*
====================
qyCurve_KochanekBartelsSpline::AddValue

add a timed/value pair to the spline
returns the index to the inserted pair
====================
*/
template< typename type >
QY_INLINE int qyCurve_KochanekBartelsSpline<type>::AddValue( const float time, const type &value ) {
    int index;

    index = IndexForTime( time );
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.insert( it, time );
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.insert( it, value );
    }
    {
        qyArray<float>::iterator it = tension.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        tension.insert( it, 0.0f );
    }
    {
        qyArray<float>::iterator it = continuity.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        continuity.insert( it, 0.0f );
    }
    {
        qyArray<float>::iterator it = bias.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        bias.insert( it, 0.0f );
    }
    return index;
}

/*
====================
qyCurve_KochanekBartelsSpline::AddValue

add a timed/value pair to the spline
returns the index to the inserted pair
====================
*/
template< typename type >
QY_INLINE int qyCurve_KochanekBartelsSpline<type>::AddValue( const float time, const type &value, const float tension, const float continuity, const float bias ) {
    int index;

    index = IndexForTime( time );
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.insert( it, time );
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.insert( it, value );
    }
    {
        qyArray<float>::iterator it = this->tension.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        this->tension.insert( it, tension);
    }
    {
        qyArray<float>::iterator it = this->continuity.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        this->continuity.insert( it, continuity );
    }
    {
        qyArray<float>::iterator it = this->bias.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        this->bias.insert( it, bias );
    }
    return index;
}

/*
====================
qyCurve_KochanekBartelsSpline::GetCurrentValue

get the value for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_KochanekBartelsSpline<type>::GetCurrentValue( const float time ) const {
    int i;
    float bvals[4], clampedTime;
    type v, t0, t1;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    TangentsForIndex( i - 1, t0, t1 );
    Basis( i - 1, clampedTime, bvals );
    v = bvals[0] * ValueForIndex( i - 1 );
    v += bvals[1] * ValueForIndex( i );
    v += bvals[2] * t0;
    v += bvals[3] * t1;
    return v;
}

/*
====================
qyCurve_KochanekBartelsSpline::GetCurrentFirstDerivative

get the first derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_KochanekBartelsSpline<type>::GetCurrentFirstDerivative( const float time ) const {
    int i;
    float bvals[4], d, clampedTime;
    type v, t0, t1;

    if ( this->times.size() == 1 ) {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    TangentsForIndex( i - 1, t0, t1 );
    BasisFirstDerivative( i - 1, clampedTime, bvals );
    v = bvals[0] * ValueForIndex( i - 1 );
    v += bvals[1] * ValueForIndex( i );
    v += bvals[2] * t0;
    v += bvals[3] * t1;
    d = ( TimeForIndex( i ) - TimeForIndex( i-1 ) );
    return v / d;
}

/*
====================
qyCurve_KochanekBartelsSpline::GetCurrentSecondDerivative

get the second derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_KochanekBartelsSpline<type>::GetCurrentSecondDerivative( const float time ) const {
    int i;
    float bvals[4], d, clampedTime;
    type v, t0, t1;

    if ( this->times.size() == 1 ) {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    TangentsForIndex( i - 1, t0, t1 );
    BasisSecondDerivative( i - 1, clampedTime, bvals );
    v = bvals[0] * ValueForIndex( i - 1 );
    v += bvals[1] * ValueForIndex( i );
    v += bvals[2] * t0;
    v += bvals[3] * t1;
    d = ( TimeForIndex( i ) - TimeForIndex( i-1 ) );
    return v / ( d * d );
}

/*
====================
qyCurve_KochanekBartelsSpline::TangentsForIndex
====================
*/
template< typename type >
QY_INLINE void qyCurve_KochanekBartelsSpline<type>::TangentsForIndex( const int index, type &t0, type &t1 ) const {
    float dt, omt, omc, opc, omb, opb, adj, s0, s1;
    type delta;

    delta = ValueForIndex( index + 1 ) - ValueForIndex( index );
    dt = TimeForIndex( index + 1 ) - TimeForIndex( index );

    omt = 1.0f - tension[index];
    omc = 1.0f - continuity[index];
    opc = 1.0f + continuity[index];
    omb = 1.0f - bias[index];
    opb = 1.0f + bias[index];
    adj = 2.0f * dt / ( TimeForIndex( index + 1 ) - TimeForIndex( index - 1 ) );
    s0 = 0.5f * adj * omt * opc * opb;
    s1 = 0.5f * adj * omt * omc * omb;

    // outgoing tangent at first point
    t0 = s1 * delta + s0 * ( ValueForIndex( index ) - ValueForIndex( index - 1 ) );

    omt = 1.0f - tension[index + 1];
    omc = 1.0f - continuity[index + 1];
    opc = 1.0f + continuity[index + 1];
    omb = 1.0f - bias[index + 1];
    opb = 1.0f + bias[index + 1];
    adj = 2.0f * dt / ( TimeForIndex( index + 2 ) - TimeForIndex( index ) );
    s0 = 0.5f * adj * omt * omc * opb;
    s1 = 0.5f * adj * omt * opc * omb;

    // incoming tangent at second point
    t1 = s1 * ( ValueForIndex( index + 2 ) - ValueForIndex( index + 1 ) ) + s0 * delta;
}

/*
====================
qyCurve_KochanekBartelsSpline::Basis

spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_KochanekBartelsSpline<type>::Basis( const int index, const float t, float *bvals ) const {
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = ( ( 2.0f * s - 3.0f ) * s ) * s + 1.0f;				// 2.0f * s * s * s - 3.0f * s * s + 1.0f
    bvals[1] = ( ( -2.0f * s + 3.0f ) * s ) * s;					// -2.0f * s * s * s + 3.0f * s * s
    bvals[2] = ( ( s - 2.0f ) * s ) * s + s;						// s * s * s - 2.0f * s * s + s
    bvals[3] = ( ( s - 1.0f ) * s ) * s;							// s * s * s - s * s
}

/*
====================
qyCurve_KochanekBartelsSpline::BasisFirstDerivative

first derivative of spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_KochanekBartelsSpline<type>::BasisFirstDerivative( const int index, const float t, float *bvals ) const {
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = ( 6.0f * s - 6.0f ) * s;								// 6.0f * s * s - 6.0f * s
    bvals[1] = ( -6.0f * s + 6.0f ) * s;							// -6.0f * s * s + 6.0f * s
    bvals[2] = ( 3.0f * s - 4.0f ) * s + 1.0f;						// 3.0f * s * s - 4.0f * s + 1.0f
    bvals[3] = ( 3.0f * s - 2.0f ) * s;								// 3.0f * s * s - 2.0f * s
}

/*
====================
qyCurve_KochanekBartelsSpline::BasisSecondDerivative

second derivative of spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_KochanekBartelsSpline<type>::BasisSecondDerivative( const int index, const float t, float *bvals ) const {
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = 12.0f * s - 6.0f;
    bvals[1] = -12.0f * s + 6.0f;
    bvals[2] = 6.0f * s - 4.0f;
    bvals[3] = 6.0f * s - 2.0f;
}

/*
===============================================================================

B-Spline base template. Uses recursive definition and is slow.
Use qyCurve_UniformCubicBSpline or qyCurve_NonUniformBSpline instead.

===============================================================================
*/

template< typename type >
class qyCurve_BSpline : public qyCurve_Spline<type> {
public:
    qyCurve_BSpline( void );

    virtual int			GetOrder( void ) const { return order; }
    virtual void		SetOrder( const int i ) { assert( i > 0 && i < 10 ); order = i; }

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    int					order;

    float				Basis( const int index, const int order, const float t ) const;
    float				BasisFirstDerivative( const int index, const int order, const float t ) const;
    float				BasisSecondDerivative( const int index, const int order, const float t ) const;
};

/*
====================
qyCurve_BSpline::qyCurve_NaturalCubicSpline
====================
*/
template< typename type >
QY_INLINE qyCurve_BSpline<type>::qyCurve_BSpline( void ) {
    order = 4;	// default to cubic
}

/*
====================
qyCurve_BSpline::GetCurrentValue

get the value for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_BSpline<type>::GetCurrentValue( const float time ) const {
    int i, j, k;
    float clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < order; j++ ) {
        k = i + j - (order>>1);
        v += Basis( k-2, order, clampedTime ) * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_BSpline::GetCurrentFirstDerivative

get the first derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_BSpline<type>::GetCurrentFirstDerivative( const float time ) const {
    int i, j, k;
    float clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < order; j++ ) {
        k = i + j - (order>>1);
        v += BasisFirstDerivative( k-2, order, clampedTime ) * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_BSpline::GetCurrentSecondDerivative

get the second derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_BSpline<type>::GetCurrentSecondDerivative( const float time ) const {
    int i, j, k;
    float clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < order; j++ ) {
        k = i + j - (order>>1);
        v += BasisSecondDerivative( k-2, order, clampedTime ) * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_BSpline::Basis

spline basis function
====================
*/
template< typename type >
QY_INLINE float qyCurve_BSpline<type>::Basis( const int index, const int order, const float t ) const {
    if ( order <= 1 ) {
        if ( TimeForIndex( index ) < t && t <= TimeForIndex( index + 1 ) ) {
            return 1.0f;
        } else {
            return 0.0f;
        }
    } else {
        float sum = 0.0f;
        float d1 = TimeForIndex( index+order-1 ) - TimeForIndex( index );
        if ( d1 != 0.0f ) {
            sum += (float) ( t - TimeForIndex( index ) ) * Basis( index, order-1, t ) / d1;
        }

        float d2 = TimeForIndex( index+order ) - TimeForIndex( index+1 );
        if ( d2 != 0.0f ) {
            sum += (float) ( TimeForIndex( index+order ) - t ) * Basis( index+1, order-1, t ) / d2;
        }
        return sum;
    }
}

/*
====================
qyCurve_BSpline::BasisFirstDerivative

first derivative of spline basis function
====================
*/
template< typename type >
QY_INLINE float qyCurve_BSpline<type>::BasisFirstDerivative( const int index, const int order, const float t ) const {
    return ( Basis( index, order-1, t ) - Basis( index+1, order-1, t ) ) *
        (float) ( order - 1 ) / ( TimeForIndex( index + ( order - 1 ) - 2 ) - TimeForIndex( index - 2 ) );
}

/*
====================
qyCurve_BSpline::BasisSecondDerivative

second derivative of spline basis function
====================
*/
template< typename type >
QY_INLINE float qyCurve_BSpline<type>::BasisSecondDerivative( const int index, const int order, const float t ) const {
    return ( BasisFirstDerivative( index, order-1, t ) - BasisFirstDerivative( index+1, order-1, t ) ) *
        (float) ( order - 1 ) / ( TimeForIndex( index + ( order - 1 ) - 2 ) - TimeForIndex( index - 2 ) );
}

/*
===============================================================================

Uniform Non-Rational Cubic B-Spline template.

===============================================================================
*/

template< typename type >
class qyCurve_UniformCubicBSpline : public qyCurve_BSpline<type> {
public:
    qyCurve_UniformCubicBSpline( void );

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    void				Basis( const int index, const float t, float *bvals ) const;
    void				BasisFirstDerivative( const int index, const float t, float *bvals ) const;
    void				BasisSecondDerivative( const int index, const float t, float *bvals ) const;
};

/*
====================
qyCurve_UniformCubicBSpline::qyCurve_UniformCubicBSpline
====================
*/
template< typename type >
QY_INLINE qyCurve_UniformCubicBSpline<type>::qyCurve_UniformCubicBSpline( void ) {
    this->order = 4;	// always cubic
}

/*
====================
qyCurve_UniformCubicBSpline::GetCurrentValue

get the value for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_UniformCubicBSpline<type>::GetCurrentValue( const float time ) const {
    int i, j, k;
    float bvals[4], clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    Basis( i-1, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < 4; j++ ) {
        k = i + j - 2;
        v += bvals[j] * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_UniformCubicBSpline::GetCurrentFirstDerivative

get the first derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_UniformCubicBSpline<type>::GetCurrentFirstDerivative( const float time ) const {
    int i, j, k;
    float bvals[4], d, clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    BasisFirstDerivative( i-1, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < 4; j++ ) {
        k = i + j - 2;
        v += bvals[j] * ValueForIndex( k );
    }
    d = ( TimeForIndex( i ) - TimeForIndex( i-1 ) );
    return v / d;
}

/*
====================
qyCurve_UniformCubicBSpline::GetCurrentSecondDerivative

get the second derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_UniformCubicBSpline<type>::GetCurrentSecondDerivative( const float time ) const {
    int i, j, k;
    float bvals[4], d, clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    BasisSecondDerivative( i-1, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < 4; j++ ) {
        k = i + j - 2;
        v += bvals[j] * ValueForIndex( k );
    }
    d = ( TimeForIndex( i ) - TimeForIndex( i-1 ) );
    return v / ( d * d );
}

/*
====================
qyCurve_UniformCubicBSpline::Basis

spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_UniformCubicBSpline<type>::Basis( const int index, const float t, float *bvals ) const {
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = ( ( ( -s + 3.0f ) * s - 3.0f ) * s + 1.0f ) * ( 1.0f / 6.0f );
    bvals[1] = ( ( ( 3.0f * s - 6.0f ) * s ) * s + 4.0f ) * ( 1.0f / 6.0f );
    bvals[2] = ( ( ( -3.0f * s + 3.0f ) * s + 3.0f ) * s + 1.0f ) * ( 1.0f / 6.0f );
    bvals[3] = ( s * s * s ) * ( 1.0f / 6.0f );
}

/*
====================
qyCurve_UniformCubicBSpline::BasisFirstDerivative

first derivative of spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_UniformCubicBSpline<type>::BasisFirstDerivative( const int index, const float t, float *bvals ) const {
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = -0.5f * s * s + s - 0.5f;
    bvals[1] = 1.5f * s * s - 2.0f * s;
    bvals[2] = -1.5f * s * s + s + 0.5f;
    bvals[3] = 0.5f * s * s;
}

/*
====================
qyCurve_UniformCubicBSpline::BasisSecondDerivative

second derivative of spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_UniformCubicBSpline<type>::BasisSecondDerivative( const int index, const float t, float *bvals ) const {
    float s = (float) ( t - TimeForIndex( index ) ) / ( TimeForIndex( index+1 ) - TimeForIndex( index ) );
    bvals[0] = -s + 1.0f;
    bvals[1] = 3.0f * s - 2.0f;
    bvals[2] = -3.0f * s + 1.0f;
    bvals[3] = s;
}


/*
===============================================================================

Non-Uniform Non-Rational B-Spline (NUBS) template.

===============================================================================
*/

template< typename type >
class qyCurve_NonUniformBSpline : public qyCurve_BSpline<type> {
public:
    qyCurve_NonUniformBSpline( void );

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    void				Basis( const int index, const int order, const float t, float *bvals ) const;
    void				BasisFirstDerivative( const int index, const int order, const float t, float *bvals ) const;
    void				BasisSecondDerivative( const int index, const int order, const float t, float *bvals ) const;
};

/*
====================
qyCurve_NonUniformBSpline::qyCurve_NonUniformBSpline
====================
*/
template< typename type >
QY_INLINE qyCurve_NonUniformBSpline<type>::qyCurve_NonUniformBSpline( void ) {
}

/*
====================
qyCurve_NonUniformBSpline::GetCurrentValue

get the value for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_NonUniformBSpline<type>::GetCurrentValue( const float time ) const {
    int i, j, k;
    float clampedTime;
    type v;
    float *bvals = (float *) _alloca( this->order * sizeof(float) );

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    Basis( i-1, this->order, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < this->order; j++ ) {
        k = i + j - (this->order>>1);
        v += bvals[j] * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_NonUniformBSpline::GetCurrentFirstDerivative

get the first derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_NonUniformBSpline<type>::GetCurrentFirstDerivative( const float time ) const {
    int i, j, k;
    float clampedTime;
    type v;
    float *bvals = (float *) _alloca( this->order * sizeof(float) );

    if ( this->times.size() == 1 ) {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    BasisFirstDerivative( i-1, this->order, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < this->order; j++ ) {
        k = i + j - (this->order>>1);
        v += bvals[j] * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_NonUniformBSpline::GetCurrentSecondDerivative

get the second derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_NonUniformBSpline<type>::GetCurrentSecondDerivative( const float time ) const {
    int i, j, k;
    float clampedTime;
    type v;
    float *bvals = (float *) _alloca( this->order * sizeof(float) );

    if ( this->times.size() == 1 ) {
        return ( this->values[0] - this->values[0] );
    }

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    BasisSecondDerivative( i-1, this->order, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    for ( j = 0; j < this->order; j++ ) {
        k = i + j - (this->order>>1);
        v += bvals[j] * ValueForIndex( k );
    }
    return v;
}

/*
====================
qyCurve_NonUniformBSpline::Basis

spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_NonUniformBSpline<type>::Basis( const int index, const int order, const float t, float *bvals ) const {
    int r, s, i;
    float omega;

    bvals[order-1] = 1.0f;
    for ( r = 2; r <= order; r++ ) {
        i = index - r + 1;
        bvals[order - r] = 0.0f;
        for ( s = order - r + 1; s < order; s++ ) {
            i++;
            omega = (float) ( t - TimeForIndex( i ) ) / ( TimeForIndex( i + r - 1 ) - TimeForIndex( i ) );
            bvals[s - 1] += ( 1.0f - omega ) * bvals[s];
            bvals[s] *= omega;
        }
    }
}

/*
====================
qyCurve_NonUniformBSpline::BasisFirstDerivative

first derivative of spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_NonUniformBSpline<type>::BasisFirstDerivative( const int index, const int order, const float t, float *bvals ) const {
    int i;

    Basis( index, order-1, t, bvals+1 );
    bvals[0] = 0.0f;
    for ( i = 0; i < order-1; i++ ) {
        bvals[i] -= bvals[i+1];
        bvals[i] *= (float) ( order - 1) / ( TimeForIndex( index + i + (order-1) - 2 ) - TimeForIndex( index + i - 2 ) );
    }
    bvals[i] *= (float) ( order - 1) / ( TimeForIndex( index + i + (order-1) - 2 ) - TimeForIndex( index + i - 2 ) );
}

/*
====================
qyCurve_NonUniformBSpline::BasisSecondDerivative

second derivative of spline basis functions
====================
*/
template< typename type >
QY_INLINE void qyCurve_NonUniformBSpline<type>::BasisSecondDerivative( const int index, const int order, const float t, float *bvals ) const {
    int i;

    BasisFirstDerivative( index, order-1, t, bvals+1 );
    bvals[0] = 0.0f;
    for ( i = 0; i < order-1; i++ ) {
        bvals[i] -= bvals[i+1];
        bvals[i] *= (float) ( order - 1) / ( TimeForIndex( index + i + (order-1) - 2 ) - TimeForIndex( index + i - 2 ) );
    }
    bvals[i] *= (float) ( order - 1) / ( TimeForIndex( index + i + (order-1) - 2 ) - TimeForIndex( index + i - 2 ) );
}


/*
===============================================================================

Non-Uniform Rational B-Spline (NURBS) template.

===============================================================================
*/

template< typename type >
class qyCurve_NURBS : public qyCurve_NonUniformBSpline<type> {
public:
    qyCurve_NURBS( void );

    virtual int			AddValue( const float time, const type &value );
    virtual int			AddValue( const float time, const type &value, const float weight );
    virtual void		RemoveIndex( const int index );
    virtual void		Clear( void ) { this->values.clear(); this->times.clear(); weights.clear(); this->currentIndex = -1; }

    virtual type		GetCurrentValue( const float time ) const;
    virtual type		GetCurrentFirstDerivative( const float time ) const;
    virtual type		GetCurrentSecondDerivative( const float time ) const;

protected:
    qyArray<float>		weights;

    float				WeightForIndex( const int index ) const;
};

/*
====================
qyCurve_NURBS::qyCurve_NURBS
====================
*/
template< typename type >
QY_INLINE qyCurve_NURBS<type>::qyCurve_NURBS( void ) {
}

//-----------------------------------------------------------------------
template< typename type >
QY_INLINE void qyCurve_NURBS<type>::RemoveIndex( const int index )
{
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.erase( it );
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.erase( it );
    }
    {
        qyArray<float>::iterator it = weights.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        weights.erase( it );
    }
}

/*
====================
qyCurve_NURBS::AddValue

add a timed/value pair to the spline
returns the index to the inserted pair
====================
*/
template< typename type >
QY_INLINE int qyCurve_NURBS<type>::AddValue( const float time, const type &value ) {
    int index;

    index = IndexForTime( time );
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.insert( it, time );
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.insert( it, value );
    }
    {
        qyArray<float>::iterator it = weights.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        weights.insert( it, 1.0f );
    }
    return index;
}

/*
====================
qyCurve_NURBS::AddValue

add a timed/value pair to the spline
returns the index to the inserted pair
====================
*/
template< typename type >
QY_INLINE int qyCurve_NURBS<type>::AddValue( const float time, const type &value, const float weight ) {
    int index;

    index = IndexForTime( time );
    {
        TimeList::iterator it = times.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        times.insert( it, time );
    }
    {
        ValueList::iterator it = values.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        values.insert( it, value );
    }
    {
        qyArray<float>::iterator it = weights.begin();
        for ( int i = 0; i < index; i++ )
        {
            ++it;
        }
        weights.insert( it, weight );
    }
    return index;
}

/*
====================
qyCurve_NURBS::GetCurrentValue

get the value for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_NURBS<type>::GetCurrentValue( const float time ) const {
    int i, j, k;
    float w, b, *bvals, clampedTime;
    type v;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    bvals = (float *) _alloca( this->order * sizeof(float) );

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    Basis( i-1, this->order, clampedTime, bvals );
    v = this->values[0] - this->values[0];
    w = 0.0f;
    for ( j = 0; j < this->order; j++ ) {
        k = i + j - (this->order>>1);
        b = bvals[j] * WeightForIndex( k );
        w += b;
        v += b * ValueForIndex( k );
    }
    return v / w;
}

/*
====================
qyCurve_NURBS::GetCurrentFirstDerivative

get the first derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_NURBS<type>::GetCurrentFirstDerivative( const float time ) const {
    int i, j, k;
    float w, wb, wd1, b, d1, *bvals, *d1vals, clampedTime;
    type v, vb, vd1;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    bvals = (float *) _alloca( this->order * sizeof(float) );
    d1vals = (float *) _alloca( this->order * sizeof(float) );

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    Basis( i-1, this->order, clampedTime, bvals );
    BasisFirstDerivative( i-1, this->order, clampedTime, d1vals );
    vb = vd1 = this->values[0] - this->values[0];
    wb = wd1 = 0.0f;
    for ( j = 0; j < this->order; j++ ) {
        k = i + j - (this->order>>1);
        w = WeightForIndex( k );
        b = bvals[j] * w;
        d1 = d1vals[j] * w;
        wb += b;
        wd1 += d1;
        v = ValueForIndex( k );
        vb += b * v;
        vd1 += d1 * v;
    }
    return ( wb * vd1 - vb * wd1 ) / ( wb * wb );
}

/*
====================
qyCurve_NURBS::GetCurrentSecondDerivative

get the second derivative for the given time
====================
*/
template< typename type >
QY_INLINE type qyCurve_NURBS<type>::GetCurrentSecondDerivative( const float time ) const {
    int i, j, k;
    float w, wb, wd1, wd2, b, d1, d2, *bvals, *d1vals, *d2vals, clampedTime;
    type v, vb, vd1, vd2;

    if ( this->times.size() == 1 ) {
        return this->values[0];
    }

    bvals = (float *) _alloca( this->order * sizeof(float) );
    d1vals = (float *) _alloca( this->order * sizeof(float) );
    d2vals = (float *) _alloca( this->order * sizeof(float) );

    clampedTime = ClampedTime( time );
    i = IndexForTime( clampedTime );
    Basis( i-1, this->order, clampedTime, bvals );
    BasisFirstDerivative( i-1, this->order, clampedTime, d1vals );
    BasisSecondDerivative( i-1, this->order, clampedTime, d2vals );
    vb = vd1 = vd2 = this->values[0] - this->values[0];
    wb = wd1 = wd2 = 0.0f;
    for ( j = 0; j < this->order; j++ ) {
        k = i + j - (this->order>>1);
        w = WeightForIndex( k );
        b = bvals[j] * w;
        d1 = d1vals[j] * w;
        d2 = d2vals[j] * w;
        wb += b;
        wd1 += d1;
        wd2 += d2;
        v = ValueForIndex( k );
        vb += b * v;
        vd1 += d1 * v;
        vd2 += d2 * v;
    }
    return ( ( wb * wb ) * ( wb * vd2 - vb * wd2 ) - ( wb * vd1 - vb * wd1 ) * 2.0f * wb * wd1 ) / ( wb * wb * wb * wb );
}

/*
====================
qyCurve_NURBS::WeightForIndex

get the weight for the given index
====================
*/
template< typename type >
QY_INLINE float qyCurve_NURBS<type>::WeightForIndex( const int index ) const {
    int n = (int)weights.size()-1;

    if ( index < 0 ) {
        if ( this->boundaryType == this->BT_CLOSED ) {
            return weights[ weights.size() + index % weights.size() ];
        } else {
            return weights[0] + index * ( weights[1] - weights[0] );
        }
    } else if ( index > n ) {
        if ( this->boundaryType == this->BT_CLOSED ) {
            return weights[ index % weights.size() ];
        } else {
            return weights[n] + ( index - n ) * ( weights[n] - weights[n-1] );
        }
    }
    return weights[index];
}

#endif
